#include "rclcpp/rclcpp.hpp"
#include "face_interfaces/srv/partol.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/msg/parameter_value.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "rcl_interfaces/msg/parameter_type.hpp"
#include <chrono>

using namespace std::chrono_literals;
using Partol=face_interfaces::srv::Partol;
using Parameter=rcl_interfaces::msg::Parameter;
using ParameterValue=rcl_interfaces::msg::ParameterValue;
using ParameterType=rcl_interfaces::msg::ParameterType;
using SetParameters=rcl_interfaces::srv::SetParameters;

class PartolClient :public rclcpp::Node
{
    private:
        rclcpp::TimerBase::SharedPtr timer_;
        rclcpp::Client<Partol>::SharedPtr client_;

    public:
        // explicit防止隐式转换
        explicit PartolClient(const std::string& node_name ):Node(node_name)
        {
            client_=this->create_client<Partol>("partol");
            timer_=this->create_wall_timer(10s,[&]()->void{
                // 检测服务是否上下
                while (!this->client_->wait_for_service(1s))
                {
                    if(!rclcpp::ok())
                    {
                        RCLCPP_ERROR(this->get_logger(),"等待服务中，rclcpp挂掉");
                        return ;
                    }
                    RCLCPP_INFO(this->get_logger(),"等待上线");
                    // 初始化随机数种子
                    srand(time(NULL));

                }
                // 构造请求对象
                auto request = std::make_shared<Partol::Request>();
                request->target_x=rand()%15 ;
                request->target_y=rand()%15 ;
                RCLCPP_INFO(this->get_logger(),"准备好目标点%f，%f",request->target_x,request->target_y);

                // 发送请求
                this->client_->async_send_request(request,[&](rclcpp::Client<Partol>::SharedFuture result_funture)->void{
                    auto response=result_funture.get();
                    if(response->result==Partol::Response::SUCESS){
                        RCLCPP_INFO(this->get_logger(),"请求目标成功");
                    }
                    if(response->result==Partol::Response::FAIL){
                        RCLCPP_INFO(this->get_logger(),"请求目标失败");
                    }
                });


            });
        }

        //创建客户端发送请求，返回结果
        SetParameters::Response::SharedPtr call_set_parameter(Parameter &param)
        {
            auto param_client_=this->create_client<SetParameters>("/TurtirControl/set_parameters");
           
            // 检测服务是否上下
            while (!param_client_->wait_for_service(1s))
            {
                if(!rclcpp::ok())
                {
                    RCLCPP_ERROR(this->get_logger(),"等待服务中，rclcpp挂掉");
                    return nullptr;
                }
                RCLCPP_INFO(this->get_logger(),"等待上线");
                // 初始化随机数种子
                srand(time(NULL));

            }
            // 构造请求对象
            auto request = std::make_shared<SetParameters::Request>();
            request->parameters.push_back(param);

            // 发送请求
            auto furture=param_client_->async_send_request(request);
            rclcpp::spin_until_future_complete(this->get_node_base_interface(),furture);
            auto response=furture.get();
            return response;
            
        }

        // 更新参数
        void update_server_param_k(double k)
        {
            // 创建参数对象
            auto param=Parameter();
            param.name="k";

            auto param_value=ParameterValue();
            param_value.type=ParameterType::PARAMETER_DOUBLE;

            param_value.double_value=k;
            param.value=param_value;

            auto response=this->call_set_parameter(param);
            if (response==NULL)
            {
                RCLCPP_INFO(this->get_logger(),"参数更新失败 ");

                return ;
            }
            for (auto result:response->results)
            {
                if(result.successful==false)
                {
                    RCLCPP_INFO(this->get_logger(),"参数更新失败,%s",result.reason.c_str());
                }
                else
                {
                    RCLCPP_INFO(this->get_logger(),"参数更新成功 ");
                }
            }

        }
    
      
};

int main(int argc, char** argv)
{
    rclcpp::init(argc,argv);
    auto node =std::make_shared<PartolClient> ("PartolClient");
    node->update_server_param_k(3.0);
    rclcpp::spin(node);

    rclcpp::shutdown();
    return 0;
}